#!/usr/bin/env python
# coding=utf-8

import rospy
from sensor_msgs.msg import Image
import cv2
import cv_bridge
import numpy as np
from geometry_msgs.msg import Twist
from math import pi, isnan
from collections import deque
from pid_msgs.msg import point_center
from pid_msgs.srv import Num_params
from pid_msgs.msg import Show_param
import time
import math
from nav_msgs.msg import Odometry

last_control = 0

# count_curve = 0
prev_centers = []
time_in = 0.0
time_out = 0.0
turn = 0.0
turn_z = 0.0

# 定义PID类
class PID:
    def __init__(self, p=0, i=0, d=0, imax=0):
        self._kp = float(p)
        self._ki = float(i)
        self._kd = float(d)
        self._imax = abs(imax)
        self._integrator = 0
        self._last_error = 0
        self._last_derivative = float('nan')
        self._last_t = 0
        self._RC = 1 / (2 * pi * 10)

    def get_pid(self, error, scaler):
        tnow = rospy.get_time()
        dt = tnow - self._last_t
        output = 0
        if self._last_t == 0 or dt > 1.0:
            dt = 0
            self.reset_I()
        self._last_t = tnow
        delta_time = float(dt)

        # 比例项
        output += error * self._kp

        # 微分项
        if abs(self._kd) > 0 and dt > 0:
            if isnan(self._last_derivative):
                derivative = 0
                self._last_derivative = 0
            else:
                derivative = (error - self._last_error) / delta_time
            derivative = self._last_derivative + \
                         ((delta_time / (self._RC + delta_time)) * \
                          (derivative - self._last_derivative))
            self._last_error = error
            self._last_derivative = derivative
            output += self._kd * derivative

        output *= scaler

        # 积分项
        if abs(self._ki) > 0 and dt > 0:
            self._integrator += (error * self._ki) * scaler * delta_time
            self._integrator = max(min(self._integrator, self._imax), -self._imax)
            output += self._integrator

        return output

    def reset_I(self):
        self._integrator = 0
        self._last_derivative = float('nan')





def find_all_black_segments(row, min_length=2):
    """
    查找数组中所有连续的黑色像素段（长度至少为min_length）

    参数:
        row: numpy数组或列表，包含图像一行的像素值
        min_length: 最小连续长度，默认为2

    返回:
        list of tuples: [(start1, end1), (start2, end2), ...] 所有符合条件的黑色段
                       如果没有符合条件的段，返回空列表
    """
    if len(row) < min_length:  # 数组长度小于最小要求，直接返回空列表
        return []

    segments = []  # 存储所有符合条件的段
    start = None  # 当前段的起始位置

    # 遍历数组
    for i in range(len(row)):
        if row[i] == 0:  # 找到黑色像素
            if start is None:  # 新段的开始
                start = i
        else:  # 找到非黑色像素
            if start is not None:  # 一个段结束了
                length = i - start
                if length >= min_length:  # 如果长度符合要求
                    segments.append((start, i - 1))
                start = None

    # 处理数组末尾的黑色段
    if start is not None:
        length = len(row) - start
        if length >= min_length:
            segments.append((start, len(row) - 1))

    if segments:
        # 按照长度排序，最长的在前
        segments.sort(key=lambda x: x[1] - x[0], reverse=True)
        # 只返回最长的一个段
        return [segments[0]]
    
    return []

def search_row_for_track_center(row, search_left, min_length=2):
    """
    在一行像素中搜索轨道中心段

    参数:
        row: numpy数组或列表，包含图像一行的像素值
        search_left: 当前搜索的起始位置
        min_length: 最小连续长度，默认为2

    返回:
        list of tuples: [(start, end)] 或 None
    """
    segments = find_all_black_segments(row, min_length)
    if not segments:
        return None
    else:
        # 获取第一个段
        start, end = segments[0]
        # 计算中心位置
        center = (start + end) // 2
        # 调整为全局坐标
        center_global = center + search_left
        return (center_global, end + search_left)

def search_pic_track(image, h, w):
    """
    在图像中搜索轨道点

    参数:
        image: 二值化的图像 (numpy数组)
        h: 图像高度
        w: 图像宽度

    返回:
        list of tuples: [(x1, y1), (x2, y2), ...] 轨道点的坐标
                       如果未找到轨道，返回 None
    """
    search_left = 0
    search_right = w - 1
    search_top = h - h // 3 - 1
    search_bot = h - 1
    track_points = []

    # 搜索最底部的一行
    row_data = image[search_bot, search_left:search_right + 1]
    segment = search_row_for_track_center(row_data, search_left)
    if segment:
        center = segment[0]
        track_points.append((center, search_bot))
    else:
        return None

    # 搜索倒数第二行
    search_left = center - 8
    search_right = center + 8
    # 确保搜索范围在图像边界内
    search_left = max(0, search_left)
    search_right = min(w - 1, search_right)
    row_data = image[search_bot - 1, search_left:search_right + 1]
    segment = search_row_for_track_center(row_data, search_left)
    if segment:
        center = segment[0]
        track_points.append((center, search_bot - 1))
    else:
        return track_points  # 如果未找到，返回已有的点

    # 继续向上搜索
    for row in range(search_bot - 2, search_top, -1):
        if len(track_points) < 2:
            # 需要至少两个点来计算 alpha
            return track_points
        # 计算 alpha: 当前行与上一行中心点的差值
        previous_center = track_points[-1][0]
        current_center = track_points[-2][0]
        alpha = previous_center - current_center
        # 计算新的搜索范围
        search_left = previous_center + alpha - 8
        search_right = previous_center + alpha + 8
        # 确保搜索范围在图像边界内
        search_left = max(0, search_left)
        search_right = min(w - 1, search_right)
        row_data = image[row, search_left:search_right + 1]
        segment = search_row_for_track_center(row_data, search_left)
        if segment:
            center = segment[0]
            track_points.append((center, row))
        else:
            break

    return track_points if track_points else None

def calculate_angle(point_1, point_2):
    delta_x = point_2[0] - point_1[0]
    delta_y = point_2[1] - point_1[1]

    angle_rad = math.atan2(delta_y, delta_x)  # 返回值范围[-π, π]
    angle_deg = math.degrees(angle_rad)
    if angle_deg < 0:
        angle_deg += 360  # 将角度转换到[0, 360)范围内
    return angle_deg

def linspace(start, end, num):
    t = int((end - start) / num)
    points = []
    for i in range(num - 1):
        point = start + i * t
        points.append(point)
    points.append(end)
    return points

def find_curve(points, tolerance_degrees=20):
    if points[0]:
        # 计算相邻点之间的角度（以度为单位）
        angles = []
        angles.append(calculate_angle(points[0], points[1]))
        angles.append(calculate_angle(points[1], points[2]))
        angles.append(calculate_angle(points[2], points[3]))
        # 比较相邻线段的角度差异是否在允许的偏差范围内
        for i in range(1, len(angles)):
            angle_diff = abs(angles[i] - angles[i-1])
            # 处理角度差异超过180度的情况
            if angle_diff > 180:
                angle_diff = 360 - angle_diff
            if angle_diff > tolerance_degrees:
                if angles[i] > 90:
                    return -1  # 左
                elif angles[i] < 90:
                    return 1  # 右
        
        return False  # 没有弧度
    else:
        return False
    
def notLine(points, tolerance=1):
    if points:
        p1, p5 = points[0], points[-1]
        # 计算直线的系数 A*x + B*y + C = 0
        # 直线通过点 p1 和 p5
        x1, y1 = p1
        x5, y5 = p5
        A = y5 - y1
        B = x1 - x5
        C = x5 * y1 - x1 * y5
        
        # 计算每个点到直线的距离
        distances = []
        for p in points:
            x, y = p
            distance = abs(A * x + B * y + C) / math.sqrt(A**2 + B**2)
            distances.append(distance)
        
        # 打印距离（可选，用于调试）
        print("点到直线的距离: {}".format(distances[0]))
        # 检查所有距离是否都小于容忍度
        for distance in distances:
            if distance > tolerance:
                return True
        return False
    return True

# 定义Foler类
class Follower:
    def __init__(self):
        self.bridge = cv_bridge.CvBridge()
        self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.image_callback)
        self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
        # self.image_pub = rospy.Publisher("/face_image", Image, queue_size=10)
        self.points_pub = rospy.Publisher("/point_center", point_center, queue_size=10)
        self.mask_pub = rospy.Publisher("/image_mask", Image, queue_size=10)
        # 定义一个服务端
        self.service = rospy.Service("/Num_params", Num_params, self.handle)
        # 发布参数
        self.pub_show_param = rospy.Publisher("/Show_param", Show_param, queue_size=10)
        # 订阅里程计数据
        self.odom_sub = rospy.Subscriber("/odom", Odometry, self.odom_callback)
        self.twist = Twist()
        self.twist.linear.x = 0
        self.twist.angular.z = 0

        # 定义一个变量，用于决定是否执行巡线代码
        self.yes_or_no = False
        # 定义一些变量，用于存储可调节参数
        self.P = 0.01
        self.I = 0.00008
        self.D = 0.018
        self.start_x = 0.1
        # self.len_q = 10
        self.window_width = 16

        # 弯道数据
        self.corner_in = None
        self.corner_turn = None
        self.corner_z = None
        self.corner_x = None
   
        


        self.pid_controller = PID(p=0.016, i=0.00008, d=0.019, imax=30)
        self.pid_controller_2 = PID(p=0.01, i=0.0002, d=0.018, imax=30)
	    # self.pid_controller = PID(p=0.016, i=0.00008, d=0.019, imax=50) 
        self.lost_counter = 0
        self.max_lost = 60
        self.prev_center_points = []
        # 队列存储最近10帧的中心误差
        self.error_queue = deque(maxlen=10)
        self.control_queue = deque(maxlen=100)  # 设置队列的最大长度，根据需要调整
        self.data_queue = deque(maxlen=10)     # 用于存储字典数据
        # self.detector = TrackCenterDetector(change_threshold=15.0)
        self.prev_centers = []
        # self.point_center = point_center
        self.frame_count = 0
        self.last_time = time.time()
        self.count_curve = 0
        # 忽略弯道检测的时间
        self.ignore_curve_until_6 = 0.0
        self.ignore_curve_until_1 = 0.0
        self.ignore_curve_until_7 = 0.0

        # 时间戳
        self.time_stamp = 0.0
        self.c = 0
        self.last_timestamp = 0.

        self.actual_x = 0.
        self.actual_z = 0.
        

    def handle(self, request):
        if request.P and request.I and request.D  and request.windows_width and request.start_x:
            self.yes_or_no = True
            self.P = request.P
            self.I = request.I
            self.D = request.D
            self.start_x = request.start_x
            # self.len_q = request.len_q
            self.window_width = request.windows_width

            # self.pid_controller = PID(self.P, self.I, self.D, imax=30)
            # self.pid_controller_2 = PID(p=0.01, i=0.0002, d=0.018, imax=30)

            self.corner_in = request.corner_in
            self.corner_turn = request.corner_turn
            self.corner_x = request.corner_x
            self.corner_z = request.corner_z

            print("corner_in: {}".format(self.corner_in))


            return True
        else:
            return False


    def odom_callback(self, odom):
        self.actual_x = odom.twist.twist.linear.x
        self.actual_z = odom.twist.twist.angular.z

    def image_callback(self, msg):
        global last_control
        # global count_curve
        global time_in
        global time_out
        global turn
        global turn_z
        # current_time = time.time()
        # timestamp = msg.header.stamp.to_sec()
        global last_control
        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
        image = cv2.resize(image, (80, 60), interpolation=cv2.INTER_AREA)
        ros_image = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")

        h, w, d = image.shape
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        
        img_h, s, v = cv2.split(hsv)

        # 对img_h 进行滤波处理
        img_h = cv2.GaussianBlur(img_h, (3, 3), 0)

        # 将图像进行自动阈值二值化处理
        _, img_binary = cv2.threshold(img_h, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
        ros_mask = self.bridge.cv2_to_imgmsg(img_binary, "mono8")


        error = 0
        avg_slope = 0
        params = Show_param()
        # timestamp = msg.header.stamp.to_sec()
      
        avg_center = None
        point_1, point_2, point_3, point_4 = None, None, None, None
        valid_centers = None
        control = 0
        # points_center = None
        # 这里需要获取客户端的请求后再决定是否开始巡线
        if self.yes_or_no:

            timestamp = rospy.get_time()
        

            if time_in < timestamp and timestamp < time_out:
                # self.twist.linear.x = 0.2
                self.twist.linear.x = self.corner_x[self.count_curve - 1]
                self.twist.angular.z = turn_z
                points_center = point_center()
                points_center.image = ros_image

                # print("curve: {} x: {} z: {}".format(self.count_curve, self.actual_x, self.actual_z))

            elif timestamp < time_in:
                self.twist.linear.x = self.corner_x[self.count_curve - 1]
                centers = search_pic_track(img_binary, h, w)
                if centers is not None:
                    self.lost_counter = 0
                    error = centers[0][0] - (w / 2)
                    self.error_queue.append(error)
                    control = self.pid_controller_2.get_pid(error, 1)
                    control = max(min(control, 1.2), -1.2)
                self.twist.angular.z = -control
                points_center = point_center()
                points_center.image = ros_image
            else:

                # cv2.imshow('mask', img_binary)
                # centers = detect_track(img_binary)
                centers = search_pic_track(img_binary, h, w)
                points_center = point_center()
                # rospy.loginfo("track_data: {}".format(track_data))
            
                # 动态划分为5个点
                num_points = 4
                points = []
                if centers is None:
                    # 如果没有中心点，全部设置为None
                    points = [None] * num_points
                elif len(centers) >= num_points:
                    # 使用numpy.linspace生成等间距的索引
                    indices = np.linspace(0, len(centers) - 1, num=num_points, dtype=int)
                    points = [centers[idx] for idx in indices]
                else:
                    # 如果中心点少于5个，重复最后一个中心点以填充
                    points = centers[:]
                    while len(points) < num_points:
                        point_final = points[-1]
                        next_start = point_final[0]
                        next_end = point_final[1]
                        next_end += 1
                        points.append((next_start, next_end))

                # 得到中心点后，计算斜率
                # slopes, angles = compute_slope(points=points)
                # 解包分段点
                if points[0]:
                    point_1, point_2, point_3, point_4 = points
                    points_center.p1 = [point_1[0], point_1[1]]
                    points_center.p2 = [point_2[0], point_2[1]]
                    points_center.p3 = [point_3[0], point_3[1]]
                    points_center.p4 = [point_4[0], point_4[1]]
                    points_center.p5 = [point_4[0], point_4[1]]

                    # 计算所有有效中心点的平均值
                    valid_centers = [c for c in centers if c[0] is not None]
                    if valid_centers:
                        avg_center_x = int(np.mean([c[0] for c in valid_centers]))
                        avg_center_y = int(np.mean([c[1] for c in valid_centers]))
                        avg_center = (avg_center_x, avg_center_y)
                    else:
                        avg_center = None

                points_center.image = ros_image

               
                
                
                # 获取当前的线速度和角速度
                linear_velocity = self.twist.linear.x
                angular_velocity = self.twist.angular.z
                # 创建字典数据
                data = {
                    'avg_center':avg_center,
                    'point_1': point_1,
                    'point_2': point_2,
                    'point_3': point_3,
                    'point_4': point_4,
                    'point_5': point_4,
                    'timestamp': timestamp,
                    'linear_velocity': linear_velocity,
                    'angular_velocity': angular_velocity
                }
                # 将字典添加到队列中
                self.data_queue.append(data)

               


                if valid_centers and avg_center is not None:

                    self.lost_counter = 0
                    # error = data['point_1'][0] - (w / 2)  # 根据需求调整偏移量
                    # error = (point_1[0] + point_2[0]) / 2 - (w/2)
                    error = data['avg_center'][0] - (w / 2)
                    self.error_queue.append(error)

                    # 使用均值误差计算PID控制
                    control = self.pid_controller.get_pid(error, 1)
                    control = max(min(control, 1.2), -1.2)
                    last_control = control
                  
                self.time_stamp = rospy.get_time()
                print("时间差: {}".format(self.time_stamp - self.last_timestamp))
                self.last_timestamp = self.time_stamp
                
                if self.time_stamp < self.ignore_curve_until_6:
                    # 2s的时间不走弯道
                    if self.count_curve == 0:
                        self.twist.linear.x = self.start_x
                    else:
                        self.twist.linear.x = 0.2
                    self.twist.angular.z = -control
                elif self.time_stamp < self.ignore_curve_until_1:
                    # 2s的时间不走弯道
                    if self.count_curve == 0:
                        self.twist.linear.x = self.start_x
                    else:
                        self.twist.linear.x = 0.2
                    self.twist.angular.z = -control
                elif self.time_stamp < self.ignore_curve_until_7:
                     # 2s的时间不走弯道
                    if self.count_curve == 0:
                        self.twist.linear.x = self.start_x
                    else:
                        self.twist.linear.x = 0.2
                    self.twist.angular.z = -control
                else:
                    curve = find_curve(points)
                    if curve:
                        print("当前有弯道")
                        if self.count_curve == 0:
                            # self.twist.linear.x = 0.5
                            self.twist.linear.x = self.start_x
                        else:
                            self.twist.linear.x = 0.2
                        
                        if self.count_curve == 0:
                            self.twist.angular.z = 0.
                            # turn_z = 0.6
                            # turn = 2.2
                            turn_z = self.corner_z[0]
                            turn = self.corner_turn[0]
                            turn_in = self.corner_in[0]
                            time_in = self.time_stamp + turn_in # 1.2
                            time_out = time_in + turn

                            self.ignore_curve_until_1 = time_out + 1.7
                        
                        elif self.count_curve == 1: 
                            self.twist.angular.z = 0.
                            # turn_z = 0.7
                            # turn = 2.5
                            turn_z = self.corner_z[1]
                            turn = self.corner_turn[1]
                            turn_in = self.corner_in[1]
                            time_in = self.time_stamp + turn_in # 1.2
                            time_out = time_in + turn

                            
                        elif self.count_curve == 2: 
                            self.twist.angular.z = 0.
                            # turn_z = 0.8
                            # turn = 2.1
                            turn_z = self.corner_z[2]
                            turn = self.corner_turn[2]
                            turn_in = self.corner_in[2]
                            time_in = self.time_stamp + turn_in # 0.8
                            time_out = time_in + turn

                        elif self.count_curve == 3: 
                            self.twist.angular.z = 0.
                            # turn_z = -0.8
                            # turn = 2.3
                            turn_z = self.corner_z[3]
                            turn = self.corner_turn[3]
                            turn_in = self.corner_in[3]
                            time_in = self.time_stamp + turn_in # 0.05
                            time_out = time_in + turn

                        elif self.count_curve == 4: 
                            self.twist.angular.z = 0.
                            # turn_z = -0.7
                            # turn = 2.3
                            turn_z = self.corner_z[4]
                            turn = self.corner_turn[4]
                            turn_in = self.corner_in[4]
                            time_in = self.time_stamp + turn_in # 1.1
                            time_out = time_in + turn

                        elif self.count_curve == 5: 
                            self.twist.angular.z = 0.
                            # turn_z = 0.8
                            # turn = 2.5
                            turn_z = self.corner_z[5]
                            turn = self.corner_turn[5]
                            turn_in = self.corner_in[5]
                            time_in = self.time_stamp + turn_in #  1.15
                            time_out = time_in + turn

                        elif self.count_curve == 6: 
                            self.twist.angular.z = 0.
                            # turn_z = 0.7
                            # turn = 2.5
                            turn_z = self.corner_z[6]
                            turn = self.corner_turn[6]
                            turn_in = self.corner_in[6]
                            time_in = self.time_stamp + turn_in # 1.1
                            time_out = time_in + turn
                            
                            # 忽略一段时间的弯道检测
                            self.ignore_curve_until_6 = time_out + 2.0

                        else: 
                            self.twist.angular.z = 0.
                            # turn_z = 0.7
                            # turn = 2.5
                            turn_z = self.corner_z[7]
                            turn = self.corner_turn[7]
                            turn_in = self.corner_in[7]
                            time_in = self.time_stamp + turn_in #1.2
                            time_out = time_in + turn

                            self.ignore_curve_until_7 = time_out + 2
                        self.count_curve += 1
                        
                        if self.count_curve == 8:
                            self.count_curve = 0
                        print(self.count_curve, curve)
                    else:
                        if self.count_curve == 0:
                            self.twist.linear.x = self.start_x
                        else:
                            self.twist.linear.x = 0.2
                        self.twist.angular.z = -control
              
            # rospy.loginfo("z: %.2f" %(self.twist.angular.z))

            self.cmd_vel_pub.publish(self.twist)
            # self.image_pub.publish(ros_image)
            self.points_pub.publish(points_center)
            self.mask_pub.publish(ros_mask)
            # 发布参数
            params.c_x = self.twist.linear.x
            params.c_z = self.twist.angular.z
            params.error = error
            params.time = rospy.get_time()
            params.slope = avg_slope
            self.pub_show_param.publish(params)


            self.frame_count += 1
            # if current_time - self.last_time >= 1:
            #         # print("FPS: {}".format(self.frame_count))
            #         self.frame_count = 0
            #         self.last_time= current_time
        else:
            print("等待请求中......")
	    #cv2.imshow("mask", mask)
        # cv2.imshow("Image", image)
        # cv2.waitKey(3)

# 初始化ROS节点
if __name__ == '__main__':
    rospy.init_node("opencv_follower")
    follower = Follower()
    rospy.spin()




